#include <TimerOne.h>


/* =====================================
 * PWM part (not using Ardu functions)
 * =====================================*/
#define MAX_PWM_DUTY 100


static struct {
  uint8_t state;              // high or low
  uint16_t freq;
  uint16_t period;
  uint16_t duration[2];       // duration of high or low
} g_pwmInfo;


void PWM_Init(uint16_t freq){
  g_pwmInfo.freq = freq;
  g_pwmInfo.period = 1000L*1000L / freq;
  Timer1.initialize(g_pwmInfo.period);
}


void PWM_Set(uint8_t duty) {
  uint32_t dura = (uint32_t)g_pwmInfo.period * duty / MAX_PWM_DUTY;
  g_pwmInfo.duration[1] = dura;  // high duration
  g_pwmInfo.duration[0] = g_pwmInfo.period - dura;
}


/* =====================================
 * BLDC part (using Ardu functions)
 * =====================================*/
#define INH_A  2
#define IN_A   3
#define INH_B  4
#define IN_B   5
#define INH_C  6
#define IN_C   7
#define HALL_A 8
#define HALL_B 9
#define HALL_C 10


const int INH[] = {INH_A, INH_B, INH_C};
const int IN[]  = {IN_A, IN_B, IN_C};
const int HALL[] = {HALL_A, HALL_B, HALL_C};

const byte steps[6][6] = {  // phase in INH_A, IN_A, INH_B, IN_B, INH_C, IN_C
  {1,1,1,0,0,0},  // AB
  {1,1,0,0,1,0},  // AC
  {0,0,1,1,1,0},  // BC
  {1,0,1,1,0,0},  // BA
  {1,0,0,0,1,1},  // CA
  {0,0,1,0,1,1}  // CB
};

byte g_inh[3], g_in[3];
byte g_idx_step = 0;


void NextStep(byte inh[], byte in[]){
  g_idx_step++;
  if (g_idx_step >= 6){
    g_idx_step = 0;
  }

  // copy state of current
  for (int i=0; i<3; i++){
    inh[i] = steps[g_idx_step][2*i];
    in[i] = steps[g_idx_step][2*i+1];
  }
}


bool IsHallChanged(){
  static byte last_hall[3] = {0};
  for (int i=0; i<3; i++) {
    byte this_hall = digitalRead(HALL[i]);
    if (this_hall != last_hall[i]){
      // only one hall changes at once, so update the changed one is enough
      last_hall[i] = this_hall;
      return true;
    }
  }
  return false;
}

void InitBLDC()
{
  for (int i=0; i<3; i++){
    pinMode(INH[i], OUTPUT);
    digitalWrite(INH[i], LOW);
    pinMode(IN[i], OUTPUT);
    digitalWrite(IN[i], LOW);
    pinMode(HALL[i], INPUT_PULLUP);
  }
}


void BLDCSet(const byte inh[], const byte in[], int dutyCycle){
  PWM_Set(dutyCycle);
  for (int i=0; i<3; i++){
    digitalWrite(IN[i], in[i]);
  }
}

static void PWM_Toggle(){  
  noInterrupts();
  uint8_t _state = ! (g_pwmInfo.state);  // toggle state
  g_pwmInfo.state = _state;
  Timer1.setPeriod(g_pwmInfo.duration[_state]);  // duration on this state
  interrupts();

  // output
  digitalWrite(INH_A, _state && g_inh[0]);
  digitalWrite(INH_B, _state && g_inh[1]);
  digitalWrite(INH_C, _state && g_inh[2]);
}


void Print_Info() {
  Serial.print("freq:"); Serial.println(g_pwmInfo.freq);
  Serial.print("state:"); Serial.println(g_pwmInfo.state);
  Serial.print("period:"); Serial.println(g_pwmInfo.period);
  Serial.print("duration:"); 
  Serial.print(g_pwmInfo.duration[0]); Serial.print(",");
  Serial.println(g_pwmInfo.duration[1]);
}


void setup() {  
  PWM_Init(2000);
  Timer1.attachInterrupt(PWM_Toggle);
  InitBLDC();

  for (int i=0; i<3; i++){
    pinMode(INH[i], OUTPUT);
    pinMode(IN[i], OUTPUT); digitalWrite(IN[i], LOW);
  }
 
  Serial.begin(115200);
  Print_Info();
}


byte g_dutyCycle = 10;
long lastTime = millis();

void loop() {
  // switch step
  if (IsHallChanged()){
    NextStep(g_inh, g_in);
    BLDCSet(g_inh, g_in, g_dutyCycle);
  }

  if ((millis() - lastTime) > 5000){
    lastTime = millis();
    // increase speed
    g_dutyCycle += 0.2 * MAX_PWM_DUTY;
    if (g_dutyCycle > 0.7*MAX_PWM_DUTY) {
      g_dutyCycle = 0.2 * MAX_PWM_DUTY;
    }
  }
}
